#pragma once

#include "WPILib.h"

class Arm {
	static const float pidPeriod = 0.05; // The rate at which the PID updates. Tune this value
										 // for better performance.
	
	static const float limitHigh = 4.9; // Maximum rotation of arm, in volts from potentiometer.
	static const float limitLow = 4.34;  // Minimum rotation of arm, in volts from potentiometer.
	
	static const float heightHigh = 120.0;
	static const float heightLow = 0.0;
	
	bool isPID;
	// isPID determines whether or not the arm is being
	// controlled with the PIDcontroller
	
	Victor motor;
	// motor is a Victor controller which outputs
	// the speed of the arm shoulder motor.
	
	AnalogChannel motorPot;
	// motorPot reads the current rotation of the shaft.
	// It reads a 12-bit value from a +/-10 volt source,
	// meaning it ranges from 0 to 4096 for -10 to +10 volts.
	
	PIDController motorControl;
	// motorControl is a PID loop which moves the arm to the
	// desired position.
	
	Solenoid wristSoleIn, wristSoleOut;
	// wristSole is the solenoid attached to the wrist. The wrist is deployed
	// when the solenoid is true.
	
	Arm(); //private default constructor
	
	float Smooth(float cur, float pre, float lim);
	// EFF: Limits the change of cur from pre to be within
	//		the provided limit.
	
public:
	Arm(int vicChannel, int potChannel, int wristChannelIn, int wristChannelOut);
	
	enum RackHeight { GROUND = 0, LOW = 36, MID = 72, HIGH = 108, SLOT = 48 };
	// Preset rack heights for ease of coding. To use, just pass
	// them to MoveTo and the type conversion will take care
	// of the rest.
	//
	// Floor = 4.34 V
	// Max = 4.76 V
	
	void Rotate(float input);
	// EFF: Rotates the arm by the specified amount.
	
	void RotateTo(float percent);
	// EFF: Rotates the arm to the specified percentage.
	
	void MoveTo(float height);
	// EFF: Moves the end of the arm to the specified height.
	
	void TogglePID(bool value);
	//EFF: Either activates or disables using PID control
	
	void TogglePID();
	//EFF: Either activates or disables using PID control
	
	void Deploy();
	
	void ToggleArm(bool value);
};
